function qs2b=getqimu2body(imufile, inillh, startTime, dt)
% Obsolte and erroeous
% Assumptions: The IMU is mounted on a vehicle that drives on the earth surface,
% Cb2imu can be determined by assuming that the body frame (Front-Right-Down)
% is related to the n-frame(NED) by only a rotation on the down axis, and by coarse
% calibration, we can determine Cimu2n, then Cimu2b
% this function requires its data are collected in static mode
alignEpoch=40;
preimudata=CQueue();    % record the previous imu data
fimu=fopen(imufile,'r');
fgetl(fimu);            % remove the header
hstream= fgetl(fimu);
mass=textscan(hstream,'%f','delimiter',',');
imudata=mass{1};
imudata(2:7,1)=imudata(2:7,1)*dt;
while(imudata(7,1)==0||imudata(1,1)<startTime)
    preimudata.push(imudata(:,end));%record previous imudata
    if(preimudata.size()>alignEpoch)
        preimudata.pop();
    end
    hstream= fgetl(fimu);
    mass=textscan(hstream,'%f','delimiter',',');
    imudata=mass{1};
    imudata(2:7,1)=imudata(2:7,1)*dt;
end
fclose(fimu);
databuf=reshape(cell2mat(preimudata.content()),7,preimudata.size());
%the method of Yuksel does not work out as well as Yudan's coarse
%alignment
%     meandat=mean(databuf(2:end,:),2)/options.dt;
%     %Compute the alignment between the geodetic and the body frame
%     [Fc wen_n wie_n g]=geoparam_v001(2, Cen(:,3), height, zeros(3,1));
%     [Cnb E err]=align_opt_v000([meandat(4:6), meandat(1:3)],[[0;0;-g], wie_n] ,[1 0]); %Note:E defined for errors on imu. Therefore, the order of vectors is important
%     qbn=cbn2quat(Cnb');
% nav(8:10) corresponds to attitude, roll, pitch, yaw of Cs2n
navdata = coarse_alignment(databuf',inillh, [],0, 3);
qs2b=att2qua([navdata(8:9),0]);% split the rotation vector
end

function test_getqimu2body()
% static data of H764G
data_h764g=[327464.052	-0.001970422	-0.003161058	-0.124572243	-2.62E-09	5.40E-06	-4.07E-09
327464.056	-0.000182975	-0.002271472	-0.12456592	5.40E-06	-5.39E-06	9.59E-09
327464.056	-0.000182975	-0.002271472	-0.12456592	5.40E-06	-5.39E-06	9.59E-09
327464.0599	-0.004654856	-0.00316057	-0.126357571	-1.08E-05	-5.40E-06	5.39E-06
327464.0638	0.000269428	-0.00404912	-0.127226456	-1.53E-10	-2.79E-10	-1.26E-11
327464.0677	-0.002866497	-0.002716955	-0.125463416	-2.62E-09	5.40E-06	-4.07E-09
327464.0716	-0.001967629	-0.004050139	-0.125459509	-1.08E-05	-5.36E-09	-1.11E-08
327464.0716	-0.001967629	-0.004050139	-0.125459509	-1.08E-05	-5.36E-09	-1.11E-08
327464.0755	0.000261602	-0.00138094	-0.126782711	5.40E-06	2.26E-09	5.51E-09
327464.0794	0.000269029	-0.004049539	-0.126338803	-1.59E-10	-2.79E-10	-1.91E-11
327464.0833	-0.004655904	-0.003161774	-0.123694614	-4.68E-09	-4.61E-09	-5.40E-06
327464.0872	-0.000182233	-0.002270659	-0.126341229	-2.62E-09	5.40E-06	-4.07E-09
327464.0911	-0.000175158	-0.004939451	-0.125452452	2.30E-09	-5.40E-06	4.03E-09
327464.0911	-0.000175158	-0.004939451	-0.125452452	2.30E-09	-5.40E-06	4.03E-09
327464.095	-0.004659372	-0.001828067	-0.124582835	-2.62E-09	5.40E-06	-4.07E-09
327464.0989	0.000268784	-0.004049978	-0.125451149	5.39E-06	-5.40E-06	-5.39E-06
327464.1028	-0.00107578	-0.003161462	-0.123681234	-1.40E-10	-2.79E-10	-1.91E-11
327464.1067	-0.003762666	-0.002271547	-0.12546696	-4.69E-09	-4.63E-09	-5.40E-06
327464.1067	-0.003762666	-0.002271547	-0.12546696	-4.69E-09	-4.63E-09	-5.40E-06
327464.1106	0.000266357	-0.00316036	-0.125894306	-5.39E-06	1.50E-09	5.39E-06
327464.1145	-0.001076451	-0.002716802	-0.125456726	-5.40E-06	-2.82E-09	-5.58E-09
327464.1184	-0.003756075	-0.004941006	-0.12280288	-5.40E-06	1.08E-05	5.38E-06
327464.1224	0.000264185	-0.002270413	-0.126782338	-5.39E-06	-1.08E-05	2.58E-09
327464.1263	0.000266487	-0.00316014	-0.126339177	-4.71E-09	-4.61E-09	-5.40E-06
327464.1263	0.000266487	-0.00316014	-0.126339177	-4.71E-09	-4.61E-09	-5.40E-06
327464.1302	-0.003764568	-0.001381041	-0.127242639	-1.53E-10	-2.65E-10	-1.91E-11
327464.1341	-0.001072034	-0.004049402	-0.12678661	5.40E-06	6.61E-09	5.40E-06
327464.138	0.000263982	-0.002270691	-0.126339558	-5.40E-06	-2.82E-09	-5.54E-09
327464.1419	-0.004654009	-0.003606617	-0.125469728	-1.53E-10	-2.65E-10	-1.91E-11
327464.1419	-0.004654009	-0.003606617	-0.125469728	-1.53E-10	-2.65E-10	-1.91E-11
327464.1458	-0.000175331	-0.004939825	-0.124564793	5.40E-06	2.27E-09	5.55E-09
327464.1497	-0.001973077	-0.002271591	-0.124572608	5.40E-06	-5.39E-06	9.55E-09
327464.1536	-0.002864526	-0.003159972	-0.127238536	-7.14E-09	5.39E-06	-5.40E-06
327464.1575	0.00026405	-0.002270721	-0.126339559	-1.53E-10	-2.79E-10	-1.91E-11
327464.1614	-0.002865198	-0.003160766	-0.125463235	-5.40E-06	-2.82E-09	-5.56E-09
327464.1614	-0.002865198	-0.003160766	-0.125463235	-5.40E-06	-2.82E-09	-5.56E-09
327464.1653	-0.001968311	-0.004050861	-0.123684211	-7.16E-09	5.39E-06	-5.40E-06
327464.1692	0.00026455	-0.002717027	-0.124564063	5.40E-06	2.29E-09	5.55E-09
327464.1731	-0.003758092	-0.00405074	-0.124578548	-1.53E-10	-2.79E-10	-4.53E-11
327464.177	-0.001074301	-0.003159608	-0.127674627	-1.59E-10	-2.79E-10	-1.91E-11
327464.177	-0.001074301	-0.003159608	-0.127674627	-1.59E-10	-2.79E-10	-1.91E-11
327464.1809	0.000263461	-0.0022712	-0.125007033	-1.53E-10	-2.79E-10	-1.91E-11
327464.1849	0.000271128	-0.004939371	-0.125450784	-4.71E-09	-4.59E-09	-5.40E-06
327464.1888	-0.003768236	-0.000492885	-0.124580057	-5.40E-06	-2.82E-09	-5.56E-09
327464.1927	-0.001070305	-0.004939725	-0.125010918	-2.62E-09	5.40E-06	-4.07E-09
327464.1966	0.000262974	-0.001826467	-0.1272274	5.40E-06	-5.39E-06	9.59E-09
327464.1966	0.000262974	-0.001826467	-0.1272274	5.40E-06	-5.39E-06	9.59E-09
327464.2005	-0.004652752	-0.004050454	-0.125469538	-2.62E-09	5.40E-06	-4.07E-09
327464.2044	-0.000188544	-4.61E-05	-0.126342172	5.40E-06	5.40E-06	5.40E-06
327464.2083	0.000271219	-0.004939405	-0.125450775	1.08E-05	-5.40E-06	-5.38E-06
327464.2122	-0.002863869	-0.0036065	-0.125463046	5.40E-06	2.27E-09	5.52E-09
327464.2122	-0.002863869	-0.0036065	-0.125463046	5.40E-06	2.27E-09	5.52E-09
327464.2161	-0.001973025	-0.002271656	-0.124572613	4.36E-09	4.05E-09	5.40E-06
327464.22	-0.000180162	-0.003160542	-0.125453202	-2.22E-09	-5.40E-06	-5.39E-06
327464.2239	-0.004657078	-0.002270691	-0.127245602	-1.53E-10	-2.65E-10	-1.91E-11
327464.2278	0.000271342	-0.004939205	-0.125893555	-1.53E-10	-2.79E-10	-1.91E-11
327464.2317	0.000264631	-0.002717036	-0.124564066	-5.40E-06	1.08E-05	5.38E-06
327464.2317	0.000264631	-0.002717036	-0.124564066	-5.40E-06	1.08E-05	5.38E-06
327464.2356	-0.003760426	-0.003161266	-0.124578927	2.31E-09	-5.40E-06	4.03E-09
327464.2395	-0.001072479	-0.004049875	-0.125898946	-2.23E-09	-5.40E-06	-5.39E-06
327464.2434	0.000262592	-0.001826809	-0.126339747	-2.62E-09	5.40E-06	-4.07E-09
327464.2474	-0.003755608	-0.004940271	-0.124578173	-5.39E-06	1.50E-09	5.39E-06
327464.2474	-0.003755608	-0.004940271	-0.124578173	-5.39E-06	1.50E-09	5.39E-06
327464.2513	-0.001074757	-0.003160013	-0.126786977	-5.40E-06	-7.15E-09	-5.40E-06
327464.2552	-0.001073869	-0.003606247	-0.125456354	-1.53E-10	-2.79E-10	-1.91E-11
327464.2591	-0.003758723	-0.004051537	-0.122803264	-4.69E-09	-4.63E-09	-5.40E-06
327464.263	0.000266512	-0.003160155	-0.126339191	-5.39E-06	-5.40E-06	-1.47E-09
327464.2669	0.000263281	-0.002271494	-0.124564259	-5.40E-06	5.39E-06	-9.59E-09
327464.2669	0.000263281	-0.002271494	-0.124564259	-5.40E-06	5.39E-06	-9.59E-09
327464.2708	-0.004654904	-0.00316051	-0.126357576	-1.08E-05	-9.69E-09	-5.41E-06
327464.2747	-0.000182612	-0.002271095	-0.125453579	5.40E-06	2.24E-09	5.53E-09
327464.2786	0.000259156	-0.000491667	-0.12634032	-5.40E-06	-7.12E-09	-5.40E-06
327464.2825	0.000271286	-0.004939281	-0.125450781	5.39E-06	-2.06E-09	-5.39E-06
327464.2825	0.000271286	-0.004939281	-0.125450781	5.39E-06	-2.06E-09	-5.39E-06
327464.2864	-0.004661938	-0.0004916	-0.127689143	1.08E-05	4.75E-09	1.11E-08
327464.2903	-0.000174567	-0.004938853	-0.12678287	5.39E-06	5.40E-06	1.47E-09
327464.2942	-0.000187701	-0.000492144	-0.125454328	1.62E-05	7.34E-09	1.66E-08
327464.2981	-0.004651042	-0.004940901	-0.123251079	5.40E-06	6.57E-09	5.40E-06
327464.302	0.000263701	-0.002270971	-0.125451914	5.39E-06	-2.06E-09	-5.39E-06
327464.302	0.000263701	-0.002270971	-0.125451914	5.39E-06	-2.06E-09	-5.39E-06
327464.3059	-0.001071136	-0.004495461	-0.125898778	5.41E-06	1.10E-08	1.08E-05
327464.3099	-0.00375836	-0.004051164	-0.123690909	5.40E-06	-5.39E-06	5.41E-06
327464.3138	0.000271914	-0.004938644	-0.127226083	-1.08E-05	-5.40E-06	-7.06E-09
327464.3177	-0.001081611	-0.000938126	-0.12501262	-5.39E-06	5.40E-06	5.39E-06
327464.3216	-0.003753851	-0.005385391	-0.125465659	-1.08E-05	-5.36E-09	-1.11E-08
327464.3216	-0.003753851	-0.005385391	-0.125465659	-1.08E-05	-5.36E-09	-1.11E-08
327464.3255	0.000270895	-0.00493973	-0.124563156	-5.39E-06	-5.40E-06	-1.51E-09
327464.3294	0.000265824	-0.003160865	-0.124563878	-1.08E-05	-5.41E-06	-5.40E-06];
data_h764g(:,2:4)= data_h764g(:,2:4)*256*0.3048;
data_h764g(:,5:7)= data_h764g(:,5:7)*256;

data_3dmgx3_35=[327599.6151	663207318	5	10611	615088000	0.03730525	0.02639442	-0.99376535	-0.00226191	0.00394306	-0.00367135
327599.6231	663207818	5	10611	623088000	0.04786717	0.02345486	-1.00133586	-0.00111382	0.00322903	-0.00091558
327599.6291	663208193	5	10611	629088000	0.04760646	0.02911424	-1.00726032	-0.00068835	0.00032555	-0.00053397
327599.6371	663208693	5	10611	637088000	0.04384981	0.02814576	-0.99888504	-0.00115849	-0.00150075	0.00339144
327599.6431	663209068	5	10611	643088000	0.04367582	0.02205516	-0.99700952	0.00101201	-0.00433716	0.00244597
327599.6511	663209568	5	10611	651088000	0.0277424	0.02371245	-1.00434756	-0.00135328	-0.00423605	-0.00396723
327599.6571	663209943	5	10611	657088000	0.01553653	0.02153745	-1.00189185	-0.00576534	0.00088917	-0.01099136
327599.6651	663210443	5	10611	665088000	0.02885437	0.02065413	-0.99647987	-0.00260251	0.00201867	-0.01134295
327599.6731	663210943	5	10611	673088000	0.03736141	0.03171034	-1.00123227	-0.00019843	-0.00007164	-0.0032498
327599.6791	663211318	5	10611	679088000	0.0357434	0.0332047	-0.99976248	-0.00580368	0.00245894	-0.00444555
327599.6871	663211818	5	10611	687088000	0.04329699	0.02570873	-0.9962095	-0.00719738	0.00212986	-0.00736876
327599.6931	663212193	5	10611	693088000	0.04600863	0.02939409	-1.00034571	-0.00459987	-0.00316544	-0.00300135
327599.7011	663212693	5	10611	701088000	0.03927551	0.02831718	-1.00370383	-0.00550483	-0.00478495	-0.00151614
327599.7071	663213068	5	10611	707088000	0.03880987	0.01884273	-0.99929768	-0.00391342	-0.00307429	0.00011709
327599.7151	663213568	5	10611	715088000	0.04095799	0.01624533	-0.99972838	-0.00147449	0.00141093	0.00128876
327599.7231	663214068	5	10611	723088000	0.03103955	0.02712839	-1.00219309	0.00097685	0.00230604	-0.00008663
327599.7311	663214568	5	10611	731088000	0.03245366	0.0236177	-0.9949187	-0.00067001	0.00441737	-0.00311156
327599.7371	663214943	5	10611	737088000	0.03706907	0.02453939	-0.99713129	0.00160971	0.00087528	-0.00504184
327599.7431	663215318	5	10611	743088000	0.03635871	0.02954417	-1.00239933	-0.00193682	-0.00170251	-0.00566619
327599.7511	663215818	5	10611	751088000	0.03601284	0.02842274	-0.99998486	-0.00622616	-0.00161298	-0.00654101
327599.7571	663216193	5	10611	757088000	0.03655317	0.02394848	-0.99777037	-0.00424428	-0.0036433	-0.00630963
327599.7651	663216693	5	10611	765088000	0.03166546	0.0276051	-1.00373161	-0.00394027	-0.00376399	-0.0068657
327599.7731	663217193	5	10611	773088000	0.03066803	0.02069255	-1.00514662	-0.00536481	-0.00100584	-0.00922957
327599.7811	663217693	5	10611	781088000	0.04061364	0.02040678	-0.99908662	-0.00164485	0.00326009	0.00027198
327599.7871	663218068	5	10611	787088000	0.04023227	0.03198768	-0.99751639	-0.00170498	0.00122518	0.00268385
327599.7931	663218443	5	10611	793088000	0.03721361	0.03466347	-0.99617898	-0.00067548	0.0010826	-0.00005462
327599.8011	663218943	5	10611	801088000	0.04176109	0.02238574	-0.99732095	0.00035357	0.00058535	-0.00062981
327599.8071	663219318	5	10611	807088000	0.04068938	0.02457514	-0.99990582	0.00197144	-0.00259659	0.00135804
327599.8151	663219818	5	10611	815088000	0.03309912	0.02544039	-1.00173545	-0.00070281	-0.00149823	-0.00304279
327599.8231	663220318	5	10611	823088000	0.03812722	0.0179265	-1.00128067	-0.00184435	0.00064407	-0.00391561
327599.8291	663220693	5	10611	829088000	0.03747042	0.02391709	-1.00430298	-0.00216636	0.00071283	-0.00547717
327599.8371	663221193	5	10611	837088000	0.02298043	0.0298847	-1.00164914	-0.00270631	0.00207784	-0.00377858
327599.8431	663221568	5	10611	843088000	0.02774559	0.02118869	-0.99451309	-0.0012699	0.00475814	0.00225927
327599.8511	663222068	5	10611	851088000	0.04799579	0.02445628	-0.99751854	0.0019587	0.00455837	-0.00272758
327599.8571	663222443	5	10611	857088000	0.04554466	0.03412692	-1.00010145	0.00416941	0.00255227	-0.00452057
327599.8651	663222943	5	10611	865088000	0.03184462	0.02690892	-0.99445099	-0.00090978	0.00319171	-0.002452
327599.8731	663223443	5	10611	873088000	0.03832641	0.02106869	-1.00053692	-0.00218524	0.0017711	0.00027897
327599.8811	663223943	5	10611	881088000	0.04053551	0.02934916	-1.0055728	-0.00228118	0.00021162	-0.00059787
327599.8871	663224318	5	10611	887088000	0.03705306	0.02616402	-0.99865419	-0.00081892	0.00160062	-0.00733424
327599.8931	663224693	5	10611	893088000	0.03623828	0.01797544	-0.99378169	0.00220669	-0.00054102	-0.00530059
327599.9011	663225193	5	10611	901088000	0.03275944	0.02224282	-1.00068247	0.00047957	-0.00220803	-0.00294791
327599.9071	663225568	5	10611	907088000	0.03113906	0.02773738	-0.99865156	-0.00147498	-0.00132785	-0.00448449
327599.9151	663226068	5	10611	915088000	0.04175115	0.02399441	-0.99465781	-0.00057629	-0.00055471	-0.0049549
327599.9231	663226568	5	10611	923088000	0.04248972	0.02580124	-1.00328767	-0.00063121	-0.00251381	-0.00495737
327599.9311	663227068	5	10611	931088000	0.03722516	0.02557651	-1.00565529	-0.00313033	-0.00211565	-0.00416782
327599.9371	663227443	5	10611	937088000	0.03668504	0.02347994	-1.00280988	-0.00302972	-0.00094175	-0.00333201
327599.9451	663227943	5	10611	945088000	0.0294678	0.03098543	-1.00319862	-0.00466256	-0.00262469	-0.0035648
327599.9531	663228443	5	10611	953088000	0.03003998	0.02690642	-1.00132906	-0.00264233	0.00199676	-0.00601476
327599.9591	663228818	5	10611	959088000	0.04037316	0.01885154	-0.99571204	-0.00042316	0.00219568	-0.00778337
327599.9671	663229318	5	10611	967088000	0.03944264	0.0256054	-0.99457562	-0.00104609	-0.00252584	-0.00141787
327599.9731	663229693	5	10611	973088000	0.03144388	0.03150137	-0.99558955	-0.0013821	-0.00106504	0.00143621
327599.9811	663230193	5	10611	981088000	0.03963152	0.02167951	-0.99722439	-0.00293375	0.00014571	-0.00104993
327599.9871	663230568	5	10611	987088000	0.04572015	0.02334289	-1.00383663	-0.00112813	-0.00136548	0.00022312
327599.9951	663231068	5	10611	995088000	0.03616637	0.03125778	-1.00649345	-0.00079609	0.00073457	-0.00079106
327600.0031	663231568	5	10612	3088000	0.0349009	0.02097148	-1.00152612	-0.00143545	0.00407337	0.00014142
327600.0091	663231943	5	10612	9088000	0.03850219	0.01706567	-1.00439382	-0.00417812	0.00011639	-0.00094208
327600.0171	663232443	5	10612	17088000	0.02864773	0.02710983	-1.00545239	-0.00513763	0.00022536	-0.00557641
327600.0231	663232818	5	10612	23088000	0.02246951	0.02572215	-0.99566066	-0.00376006	0.00309779	-0.00526396
327600.0311	663233318	5	10612	31088000	0.03744443	0.02121176	-0.99487108	-0.00134205	0.00317352	-0.00250237
327600.0371	663233693	5	10612	37088000	0.04339293	0.0284288	-0.99970543	-0.00036552	0.00055297	0.00319774];
data_3dmgx3_35 = [data_3dmgx3_35(:,1), data_3dmgx3_35(:, end-5:end-3)*9.80665, data_3dmgx3_35(:,end-2:end)];

inixyz_ant=[592574.6611  -4856604.0417   4078414.4645]';
inillh_ant=ecef2geo_v000(inixyz_ant,0);
navdata = coarse_alignment(data_h764g,inillh_ant, [],0, 3);
qs2b=att2qua([navdata(8:9),0]);
fprintf('estimated rotation from h764g to NED frame in RPY:\n'); disp(navdata(8:10));
fprintf('estimated rotation from h764g to body frame:\n'); disp(qs2b');
fprintf('expected rotation from h764g to body frame: [1, 0,0,0]\n');
navdata = coarse_alignment(data_3dmgx3_35,inillh_ant, [],0, 3);
qs2b=att2qua([navdata(8:9),0]);
fprintf('estimated rotation from 3dm-gx3-35 to NED frame in RPY:\n'); disp(navdata(8:10));
fprintf('estimated rotation from 3dm-gx3-35 to body frame:\n'); disp(qs2b');
fprintf('expected rotation from 3dm-gx3-35 to body frame: \n');
disp([0, 1, 0; -1, 0, 0; 0, 0, 1]);

end